{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# GenericProjectionFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`GenericProjectionFactor<POSE, LANDMARK, CALIBRATION>` is a versatile factor for monocular camera measurements.\n",
        "It models the reprojection error between the predicted projection of a 3D `LANDMARK` (usually `Point3`) onto the image plane of a camera defined by `POSE` (usually `Pose3`) and `CALIBRATION` (e.g., `Cal3_S2`, `Cal3Bundler`, `Cal3DS2`), and a measured 2D pixel coordinate `measured_`.\n",
        "\n",
        "Key features:\n",
        "- **Templated:** Works with different pose, landmark, and calibration types.\n",
        "- **Fixed Calibration:** Assumes the `CALIBRATION` object (`K_`) is known and fixed (passed as a shared pointer).\n",
        "- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform between the pose variable's frame (body) and the camera's sensor frame.\n",
        "- **Cheirality Handling:** Can be configured to throw an exception or return a large error if the landmark projects behind the camera.\n",
        "\n",
        "The error is the 2D vector difference:\n",
        "$$ \\text{error}(P, L) = \\text{project}(P \\cdot S, L) - z $$\n",
        "where $P$ is the pose variable, $L$ is the landmark variable, $S$ is the optional `body_P_sensor` transform, `project` is the camera projection function including calibration, and $z$ is the `measured_` point."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Pose3, Point3, Point2, Rot3, Cal3_S2, Values\n",
        "# The Python wrapper often creates specific instantiations\n",
        "from gtsam import GenericProjectionFactorCal3_S2\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X\n",
        "L = symbol_shorthand.L"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a GenericProjectionFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "Instantiate by providing:\n",
        "1. The 2D measurement (`Point2`).\n",
        "2. The noise model (typically 2D isotropic).\n",
        "3. The key for the pose variable.\n",
        "4. The key for the landmark variable.\n",
        "5. A `shared_ptr` to the fixed calibration object.\n",
        "6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n",
        "7. (Optional) Cheirality handling flags."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Factor with offset: GenericProjectionFactor, z = [\n",
            "\t330;\n",
            "\t250\n",
            "]\n",
            "  sensor pose in body frame:  R: [\n",
            "\t6.12323e-17, 6.12323e-17, 1;\n",
            "\t-1, 3.7494e-33, 6.12323e-17;\n",
            "\t-0, -1, 6.12323e-17\n",
            "]\n",
            "t: 0.1   0 0.2\n",
            "  keys = { x0 l1 }\n",
            "  noise model: unit (2) \n",
            "\n",
            "Factor without offset: GenericProjectionFactor, z = [\n",
            "\t330;\n",
            "\t250\n",
            "]\n",
            "  keys = { x0 l1 }\n",
            "  noise model: unit (2) \n"
          ]
        }
      ],
      "source": [
        "measured_pt2 = Point2(330, 250)\n",
        "pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # 1 pixel std dev\n",
        "pose_key = X(0)\n",
        "landmark_key = L(1)\n",
        "\n",
        "# Shared pointer to calibration\n",
        "K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n",
        "\n",
        "# Optional sensor pose offset\n",
        "body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n",
        "\n",
        "# Create factor with sensor offset\n",
        "factor_with_offset = GenericProjectionFactorCal3_S2(\n",
        "    measured_pt2, pixel_noise, pose_key, landmark_key, K, body_P_sensor)\n",
        "factor_with_offset.print(\"Factor with offset: \")\n",
        "\n",
        "# Create factor without sensor offset (implicitly identity)\n",
        "factor_no_offset = GenericProjectionFactorCal3_S2(\n",
        "    measured_pt2, pixel_noise, pose_key, landmark_key, K)\n",
        "factor_no_offset.print(\"\\nFactor without offset: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The error is the difference between the predicted projection and the measurement."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Error (no offset): 1175689.2145311693\n",
            "Error (with offset): 50751.576015003826\n",
            "Manual error calculation (no offset): [1370.63962025  687.55033305]\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "# Example values\n",
        "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
        "landmark = Point3(4.0, 2.0, 3.0)\n",
        "\n",
        "values.insert(pose_key, pose)\n",
        "values.insert(landmark_key, landmark)\n",
        "\n",
        "# Evaluate factor without offset\n",
        "error_no_offset = factor_no_offset.error(values)\n",
        "print(f\"Error (no offset): {error_no_offset}\")\n",
        "\n",
        "# Evaluate factor with offset\n",
        "error_with_offset = factor_with_offset.error(values)\n",
        "print(f\"Error (with offset): {error_with_offset}\")\n",
        "\n",
        "# Manually verify projection (no offset case)\n",
        "cam_no_offset = gtsam.PinholeCameraCal3_S2(pose, K)\n",
        "predicted_no_offset = cam_no_offset.project(landmark)\n",
        "manual_error = predicted_no_offset - measured_pt2\n",
        "print(f\"Manual error calculation (no offset): {manual_error}\")\n",
        "assert np.allclose(factor_no_offset.unwhitenedError(values), manual_error)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}